#include "optimizer.h"

namespace Packing
{
	Optimizer *Optimizer::instance = 0;

	Optimizer::Optimizer(std::vector<Packing_polygon*>& polygons, Polygon_triangulation& polygon_triangulation)
		: polygon_set(polygons), pt(polygon_triangulation)
	{
		//assert(!instance);
		//instance = this;
		rot_lower_bound = -boost::math::constants::pi<double>()/4.0;
		rot_upper_bound = boost::math::constants::pi<double>()/4.0;

#ifdef _CILK_
		containments.resize(nb_polytopes());
#endif
	}

	Optimizer::~Optimizer()
	{
		//instance = 0;
	}
	Optimizer::Optimization_res Optimizer::optimize_one_polytope(unsigned int id, bool enlarge, Parameter& solution)
	{
		const Polygon_triangulation::Vert_group& vg = pt.vertex_at_group(id);
		// prepare optimization 
		Containment *ctn;
#ifdef _CILK_
		containments[id].clear();
		ctn = &containments[id];
#else
		containment.clear();
		ctn = &containment;
#endif
		// use CAT temporarily
		const std::vector<Polygon_triangulation::Point> *cell_used;

		for (unsigned int i = 0; i < vg.size(); i++)
		{
			if (!enlarge)	cell_used = &vg[i]->voronoi_vtx;
			else				cell_used = &vg[i]->cat_vtx;
			const std::vector<Polygon_triangulation::Point>& vtx = *cell_used;
			if (vtx.size() <= 1)	continue;
			for (unsigned int j = 0; j < vtx.size()-1; j++)
			{
				ctn->insert_constraint(vg[i]->point(), Polygon_triangulation::Segment(vtx[j], vtx[j+1]), 0.0);
			}
		}
		ctn->centralize_constraints(polygon_set[id]->centroid());

		const unsigned int try_time_limit = 10;
		unsigned int try_times = 1;
		int knitro_res;
		while ( ((knitro_res = knitro_optimize(&solution.k, &solution.theta, &solution.tx, &solution.ty, id)) ) && try_times < try_time_limit )
		{
			double r = Geex::Numeric::random_float64();
			solution = Parameter(1.0, (rot_upper_bound-rot_lower_bound)*r+rot_lower_bound, 0.0, 0.0);
			try_times++;
		}
		
		if (try_times == try_time_limit)
			return FAIL;
		else
			return SUCCESS;
			//std::cout<<"k = "<<solution.k<<", theta = "<<solution.theta<<", tx = "<<solution.tx<<", ty = "<<solution.ty<<std::endl;
		
	}
	bool Optimizer::constrain_transformation(Constraint_type type, unsigned int id, Parameter& p)
	{
		unsigned int N = 20, n = 0; // maximal number of binary search times
		bool normal_flipped;
		const std::vector<Polygon_triangulation::Vertex_handle>& vg = pt.vertex_at_group(id);
		Packing_polygon::Point c = polygon_set[id]->centroid();
		Packing_polygon::Transformation_2 cent2origin(CGAL::TRANSLATION, Packing_polygon::Vector(c, CGAL::ORIGIN));
		Packing_polygon::Transformation_2 origin2cent(CGAL::TRANSLATION, Packing_polygon::Vector(CGAL::ORIGIN, c));
		double bf = 1.0;
		
		do
		{
			if (n == N-1)	bf = 0.0; //the last time
			switch (type)
			{
			case TRANSLATION_CONSTRAINT:
				p.tx *= bf;
				p.ty *= bf;
				break;
			case ROTATION_CONSTRAINT:
				p.theta *= bf;
				break;
			case SCALING_CONSTRAINT:
				p.k = 1.0 + (p.k - 1.0)*bf;
				break;
			}
			
			normal_flipped = false;
			double ksin = p.k*std::sin(p.theta), kcos = p.k*std::cos(p.theta);
			Packing_polygon::Transformation_2 trans(kcos, -ksin, p.tx, ksin, kcos, p.ty);
			Packing_polygon::Transformation_2 t = origin2cent*(trans*cent2origin);
			for (unsigned int i = 0; i < vg.size(); i++)
			{
				Polygon_triangulation::Face_circulator fc = vg[i]->incident_faces(), fe;
				fe = fc;
				do
				{
					if (pt.is_intergroup_face(fc))
					{
						Polygon_triangulation::Vertex_handle v0 = fc->vertex(0), v1 = fc->vertex(1), v2 = fc->vertex(2);
						Packing_polygon::Point p[3] = { v0->point(), v1->point(), v2->point() };
						if (vg[i]->group_id == v0->group_id)	p[0] = t(p[0]);
						if (vg[i]->group_id == v1->group_id)	p[1] = t(p[1]);
						if (vg[i]->group_id == v2->group_id)	p[2] = t(p[2]);
						Packing_polygon::Vector v01(p[0], p[1]), v02(p[0], p[2]);
						CGAL::Sign s = CGAL::sign(v01.x()*v02.y() - v01.y()*v02.x());
						if (fc->normal_orientation != s)
						{
							normal_flipped = true;
							break;
						}
					}
					++fc;
				}while (fc != fe);
				if (normal_flipped)
					break;
			}
			if (normal_flipped)
				bf *= 0.5;
			n++;
		} while (normal_flipped && n < N);
		return (!normal_flipped);
	}

	void Optimizer::transform_one_polytope(unsigned int id, const Parameter& p)
	{
		Packing_polygon::Point_2 c = polygon_set[id]->centroid();
		polygon_set[id]->translate(-c.x(), -c.y());
		polygon_set[id]->transform(p);
		polygon_set[id]->translate(c.x(), c.y());
		pt.transform_vertex_at_group(id, Parameter(1.0, 0.0, -c.x(), -c.y()));
		pt.transform_vertex_at_group(id, p);
		pt.transform_vertex_at_group(id, Parameter(1.0, 0.0, c.x(), c.y()));
	}

	void Optimizer::run_one_lloyd(bool enlarge)
	{
		double min_factor = 1.05;
		std::vector<Optimization_res> res(nb_polytopes());
		std::vector<Parameter> solutions(nb_polytopes());

#ifdef _CILK_
		//unsigned int i;
		cilk_for (unsigned int i = 0; i < nb_polytopes(); i++)
#else
		for (unsigned int i = 0; i < nb_polytopes(); i++)
#endif
		{
			res[i] = optimize_one_polytope(i, enlarge, solutions[i]);
			if (!enlarge)	solutions[i].k = 1.0;
		}
		double mink = std::numeric_limits<double>::max();
		for (unsigned int i = 0; i < nb_polytopes(); i++)
		{
			if (res[i] == SUCCESS)	mink = std::min(mink, solutions[i].k);
			//else
			//{
			//	solutions[i].k = 1.0;
			//	solutions[i].theta = solutions[i].tx = solutions[i].ty = 0.0;
			//}
		}
		if (mink > min_factor)
		{
			for (unsigned int i = 0; i < nb_polytopes(); i++)
				if (res[i] == SUCCESS && solutions[i].k > 1.0)	solutions[i].k = mink;
		}
		//std::cout<<"Minimum enlargement factor = "<<mink<<std::endl;
		for (unsigned int i = 0; i < nb_polytopes(); i++)
		{
			//std::cout<<solutions[i].k<<','<<solutions[i].theta<<','<<solutions[i].tx<<','<<solutions[i].ty<<std::endl;
			double current_factor = polygon_set[i]->get_scale_factor();
			if ( current_factor < 1.0 && res[i] == SUCCESS && solutions[i].k >= 1.0)
			{
				//if (enlarge)
					if (!constrain_transformation(ROTATION_CONSTRAINT, i, solutions[i]))
						if (!constrain_transformation(TRANSLATION_CONSTRAINT, i, solutions[i]))
							constrain_transformation(SCALING_CONSTRAINT, i, solutions[i]);
				if (solutions[i].k * current_factor > 1.0)
					solutions[i].k = 1.0/current_factor;
				transform_one_polytope(i, solutions[i]);
			}
			else if (current_factor >= 1.0 && res[i] == SUCCESS && solutions[i].k >= 1.0)
			{
				if (solutions[i].k > 1.0)
					solutions[i].k = 1.0;
				if (!constrain_transformation(ROTATION_CONSTRAINT, i, solutions[i]))
					constrain_transformation(TRANSLATION_CONSTRAINT, i, solutions[i]);
				transform_one_polytope(i, solutions[i]);
			}
			//std::cout<<polygon_set[i]->get_scale_factor()<<' ';
		}
		//std::cout<<std::endl;
		//glut_viewer_redraw();
	}

	int Optimizer::knitro_optimize(double* io_k, double* io_theta, double* io_t1, double* io_t2, unsigned int idx)
	{
		int  nStatus;
		/* variables that are passed to KNITRO */
		KTR_context *kc;
		int n, m, nnzJ, nnzH, objGoal, objType;
		int *cType, *jacIndexVars, *jacIndexCons;
		double obj, *x, *lambda;
		double *xLoBnds, *xUpBnds, *xInitial, *cLoBnds, *cUpBnds;
		int i, j, k; // convenience variables

		/*problem size and mem allocation */
		n = 4;
#ifdef _CILK_
		m = containments[idx].get_constraint_list_size();
#else
		m = containment.get_constraint_list_size();
#endif
		nnzJ = n*m;
		nnzH = 0;
		x = (double *)malloc(n * sizeof(double));
		lambda = (double *)malloc((m+n) * sizeof(double));

		xLoBnds      = (double *)malloc(n * sizeof(double));
		xUpBnds      = (double *)malloc(n * sizeof(double));
		xInitial     = (double *)malloc(n * sizeof(double));
		cType        = (int    *)malloc(m * sizeof(int));
		cLoBnds      = (double *)malloc(m * sizeof(double));
		cUpBnds      = (double *)malloc(m * sizeof(double));
		jacIndexVars = (int    *)malloc(nnzJ * sizeof(int));
		jacIndexCons = (int    *)malloc(nnzJ * sizeof(int));

		/* objective type */
		objType = KTR_OBJTYPE_LINEAR;
		objGoal = KTR_OBJGOAL_MAXIMIZE;
		/* bounds and constraints type */
		xLoBnds[0] = 0.0;
		xUpBnds[0] = KTR_INFBOUND;
		xLoBnds[1] = rot_lower_bound;
		xUpBnds[1] = rot_upper_bound;
		for (i = 2; i < n; i++) {
			xLoBnds[i] = -KTR_INFBOUND;
			xUpBnds[i] = KTR_INFBOUND;
		}
		for (j = 0; j < m; j++) {
			cType[j] = KTR_CONTYPE_GENERAL;
			cLoBnds[j] = 0.0;
			cUpBnds[j] = KTR_INFBOUND;
		}
		/* initial point */
		xInitial[0] = *io_k;
		xInitial[1] = *io_theta;
		xInitial[2] = *io_t1;
		xInitial[3] = *io_t2;
		/* sparsity pattern (here, of a full matrix) */
		k = 0;
		for (j = 0; j < m; j++) 
			for (i = 0; i < n; i++){
				jacIndexCons[k] = j;
				jacIndexVars[k] = i;
				k++;
			}
		/* create a KNITRO instance */
		kc = KTR_new();
		if (kc == NULL)
			return -1 ; // probably a license issue
		/* set options: automatic gradient and hessian matrix */
		if (KTR_set_int_param_by_name(kc, "gradopt", KTR_GRADOPT_EXACT) != 0)
			return -1 ;
		if (KTR_set_int_param_by_name(kc, "hessopt", KTR_HESSOPT_LBFGS) != 0)
			return -1 ;
		if (KTR_set_int_param_by_name(kc, "outlev", 0) != 0)
			return -1 ;
		if (KTR_set_int_param_by_name(kc, "maxit", 500) != 0)
			return -1 ;
		/* register the callback function */
		if (KTR_set_func_callback(kc, &callback) != 0)
			return -1 ;
		if (KTR_set_grad_callback(kc, &callback) != 0)
			return -1 ;
		/* pass the problem definition to KNITRO */
		nStatus = KTR_init_problem(kc, n, objGoal, objType, xLoBnds, xUpBnds, m, cType, cLoBnds, cUpBnds,
									nnzJ, jacIndexVars, jacIndexCons, nnzH, NULL, NULL, xInitial, NULL);
		/* free memory (KNITRO maintains its own copy) */
		free(xLoBnds);	free(xUpBnds);
		free(xInitial);
		free(cType);
		free(cLoBnds);	free(cUpBnds);
		free(jacIndexVars);	free(jacIndexCons);

		std::pair<Optimizer*, unsigned int> opti_pair(this, idx); // identify which polygon in which optimizer, just for parallelization
		nStatus = KTR_solve(kc, x, lambda, 0, &obj, NULL, NULL, NULL, NULL, NULL, &opti_pair);

		if(nStatus == 0)
		{	
			*io_k =  x[0];
			*io_theta = x[1];
			*io_t1 = x[2];
			*io_t2 = x[3];
		}

		/* delete the KNITRO instance and primal/dual solution */
		KTR_free(&kc);
		free(x);
		free(lambda);
		return nStatus;
	}

	int Optimizer::callback(const int evalRequestCode, const int n, const int m, const int nnzJ, 
							const int nnzH,	const double * const x,	const double * const lambda, 
							double * const obj, double * const c, double * const objGrad, double * const jac,
							double * const hessian,	double * const hessVector, void * userParams)
	{
		std::pair<Optimizer*, unsigned int>* opti_pair = static_cast< std::pair<Optimizer*, unsigned int>* >(userParams);
		// Optimizer* p = instance ;
		if (evalRequestCode == KTR_RC_EVALFC)
		{
			/* objective function */
			// x[0]- x[4]: k cos sin t1 t2
			*obj = x[0];
			
#ifdef _CILK_
			//p->containments[*(unsigned int*)userParams].compute_constraints(x, c, m);
			opti_pair->first->containments[opti_pair->second].compute_constraints(x, c, m);
#else
			opti_pair->first->containment.compute_constraints(x,c,m);
#endif
			return 0;
		}
		else if (evalRequestCode == KTR_RC_EVALGA)
		{
			objGrad[0] = 1.0;
			objGrad[1] = 0.0;
			objGrad[2] = 0.0;
			objGrad[3] = 0.0;

#ifdef _CILK_
			std::pair<Optimizer*, unsigned int>* opti_pair = static_cast< std::pair<Optimizer*, unsigned int>* >(userParams);
			opti_pair->first->containments[opti_pair->second].compute_constraint_grads(x, jac);
			//p->containments[*(unsigned int*)userParams].compute_constraint_grads(x, jac);
#else
			opti_pair->first->containment.compute_constraint_grads(x, jac);
#endif

			return 0;
		}
		else 
		{
			printf ("Wrong evalRequestCode in callback function.\n");
			return -1;
		}
	}
}